/*
 * platform.h
 *
 *  Created on: Dec 4, 2023
 *      Author: boen
 */

#ifndef INTERFACELIBS_PLATFORM_H_
#define INTERFACELIBS_PLATFORM_H_

#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"
#include <math.h>
#include <tim.h>
#include <stdio.h>
#include <string.h>
#include <stdint.h>
#include <stdbool.h>
#include <stdlib.h>
#include <ver.h>
#include <pid.h>


/* Exported constants --------------------------------------------------------*/

/* Exported macro ------------------------------------------------------------*/
#define DEV_ADDR											0x00u

typedef unsigned char  										byte;

#define ONEBYTE												1u
#define PI													3.1415926f

#define RAD_180												PI
#define RAD_90												1.5707963f	//(PI/2)
#define RAD_120 											2.094f

#define APP_NULL											0x00u
#define APP_F_NULL											0.0f

#define THREAD_DELAY										1u
#define TASK_THREAD_DELAY									5u
#define NOTIFY_THREAD_DELAY									100u

#define APP_TRUE											0xFFu
#define APP_FALSE											0x00u
#define APP_ENABLE											0x01u
#define APP_DISABLE											0x00u

#define TSK_RUN												0x00u
#define TSK_STOP											0x01u
#define TSK_BLOCK											0x02u
#define TSK_READY											0x03u

#define IO_OPT_READ											'R'
#define IO_OPT_WRITE										'W'
#define IO_OPT_EXEC											'E'

#define DEF_IMU												1	//0:飞迪   1:维特智能
#if DEF_IMU
#define DETA10												0
#define JY901S												1	// 需要修改参数 115200 ，6轴算法
#endif

/*=============================== 接口配置 ===============================*/
#define IMU_PORT											huart4
#define IMU_PORT_ID											UART4

#define SERVO_PORT											huart6
#define SERVO_PORT_ID 										USART6

#define DEBUG_PORT					  						huart2
#define DEBUG_PORT_ID 			  							USART2

#define SYS_TICK_TIM 		  								htim6
#define SYS_TICK_TIM_ID	  									TIM6

#define ADC_PORT_PWR										hadc1

#define CAN_PORT           									hcan1
#define CAN_PORT_EXT										hcan2
/*------------------------------- 接口配置 -------------------------------*/


/*=============================== 底盘配置 ===============================*/
#define MOTOR_DC520											1
#define MOTOR_STEP42										0

#if MOTOR_DC520
#define MOTOR_1_FB											htim5
#define MOTOR_2_FB											htim2
#define MOTOR_3_FB											htim4
#define MOTOR_4_FB											htim3

// pwm 配置
#define MOTOR_1_PWMA										htim1
#define MOTOR_1_PWMB										htim1
#define MOTOR_2_PWMA										htim1
#define MOTOR_2_PWMB										htim1
#define MOTOR_3_PWMA										htim9
#define MOTOR_3_PWMB										htim9
#define MOTOR_4_PWMA										htim11
#define MOTOR_4_PWMB										htim10

#define MOTOR_1_CHA											TIM_CHANNEL_3
#define MOTOR_1_CHB											TIM_CHANNEL_4
#define MOTOR_2_CHA											TIM_CHANNEL_2
#define MOTOR_2_CHB											TIM_CHANNEL_1
#define MOTOR_3_CHA											TIM_CHANNEL_2
#define MOTOR_3_CHB											TIM_CHANNEL_1
#define MOTOR_4_CHA											TIM_CHANNEL_1
#define MOTOR_4_CHB											TIM_CHANNEL_1

// 编码器配置
#define MOTOR_1_POA											GPIOE
#define MOTOR_1_POB											GPIOE
#define MOTOR_2_POA											GPIOE
#define MOTOR_2_POB											GPIOE
#define MOTOR_3_POA											GPIOE
#define MOTOR_3_POB											GPIOE
#define MOTOR_4_POA											GPIOB
#define MOTOR_4_POB											GPIOB

#define MOTOR_1_IOA											GPIO_PIN_13
#define MOTOR_1_IOB											GPIO_PIN_14
#define MOTOR_2_IOA											GPIO_PIN_11
#define MOTOR_2_IOB											GPIO_PIN_9
#define MOTOR_3_IOA											GPIO_PIN_6
#define MOTOR_3_IOB											GPIO_PIN_5
#define MOTOR_4_IOA											GPIO_PIN_8
#define MOTOR_4_IOB											GPIO_PIN_9

// PID配置
#define MOTOR_1_KP    										4420.0f
#define MOTOR_1_KI    										500.0f
#define MOTOR_1_KD    										0.0f

#define MOTOR_2_KP    										1620.0f
#define MOTOR_2_KI    										300.0f
#define MOTOR_2_KD    										0.0f

#define MOTOR_3_KP    										1620.0f
#define MOTOR_3_KI    										200.0f
#define MOTOR_3_KD    										0.0f

#define MOTOR_4_KP    										1620.0f
#define MOTOR_4_KI    										300.0f
#define MOTOR_4_KD    										0.0f

// 前馈配置
#define MOTOR_1_FF											315.0f
#define MOTOR_2_FF											389.0f
#define MOTOR_3_FF											357.0f
#define MOTOR_4_FF											377.0f

// 减速步进
#define MOBILE_SPD_DELTA									0.001f
#endif


// 运动参数配置
#define MOBILE_DEF_SPD										0.08f
#define MOBILE_X_AVG_SPD									0.08f
#define MOBILE_Y_AVG_SPD									0.08f
#define MOBILE_R_AVG_SPD									0.02f

#define PHY_MOTOR_MAX       								4u
#define PHY_MOTOR_1											0u
#define PHY_MOTOR_2											1u
#define PHY_MOTOR_3											2u
#define PHY_MOTOR_4											3u
/*------------------------------- 底盘配置 -------------------------------*/


/*============================== 机械臂配置 ==============================*/
#define PHY_SERVO_MAX										4u
#define PHY_SERVO_1											0u
#define PHY_SERVO_2											1u
#define PHY_SERVO_3											2u
#define PHY_SERVO_4											3u

#define SERVO_RUNTIME_MAX									2000u

#define SERVO_RX_BUFF_MAX									10u

// 机械臂配置
#define L1													0.0339f
#define L2													0.12936f
#define L3													0.12936f
#define L4													0.0415f

#define RANGE_MAX											0.2832f 	// (L2+L3+L4)^2*pi = 0.09*pi = 0.2832

#define RAD_PROP											240.0f		// (860-500)/pi/2=229.183+偏差  一弧度对应的脉冲数
#define MET_PROP											11000.0f 	// 660/0.06 一米对应的脉冲数
/*------------------------------ 机械臂配置 ------------------------------*/


/*=========================== 移动视觉抓取配置 ===========================*/
// 待抓取对象在相机中的标准显示（像素）
#define CAM_PICKUP_X										116
#define CAM_PICKUP_Y										109
#define CAM_PICKUP_W										100
#define CAM_PICKUP_H										135

#define CAM_PICKUP_X_MAX									(CAM_PICKUP_X+15)
#define CAM_PICKUP_Y_MAX									(CAM_PICKUP_Y+15)
#define CAM_PICKUP_W_MAX									(CAM_PICKUP_W+15)
#define CAM_PICKUP_H_MAX									(CAM_PICKUP_H+15)

#define CAM_PICKUP_X_MIN									(CAM_PICKUP_X-15)
#define CAM_PICKUP_Y_MIN									(CAM_PICKUP_Y-15)
#define CAM_PICKUP_W_MIN									(CAM_PICKUP_W-15)
#define CAM_PICKUP_H_MIN									(CAM_PICKUP_H-15)

// 待放置位置在相机中的标准显示（像素）
#define CAM_PLACE_X											107
#define CAM_PLACE_Y											49
#define CAM_PLACE_W											172
#define CAM_PLACE_H											104

#define CAM_PLACE_X_MAX										(CAM_PLACE_X+15)
#define CAM_PLACE_Y_MAX										(CAM_PLACE_Y+15)
#define CAM_PLACE_W_MAX										(CAM_PLACE_W+15)
#define CAM_PLACE_H_MAX										(CAM_PLACE_H+15)

#define CAM_PLACE_X_MIN										(CAM_PLACE_X-15)
#define CAM_PLACE_Y_MIN										(CAM_PLACE_Y-15)
#define CAM_PLACE_W_MIN										(CAM_PLACE_W-15)
#define CAM_PLACE_H_MIN										(CAM_PLACE_H-15)

/*--------------------------- 移动视觉抓取配置 ---------------------------*/


/* Exported types ------------------------------------------------------------*/
typedef unsigned	char									MSG_t;
typedef unsigned 	short int 								PARAM_t;
typedef unsigned 	short int 								LpPARAM_t;

typedef enum{
	bTRUE = 0x01,
	bFALSE = 0x00,
}bool_t;

typedef enum{
    SYS_FLAG_STARTUP = 0x00u,                 				// 系统启动
    SYS_FLAG_IDLE = 0x01u,                 					// 系统空闲
    SYS_FLAG_PLAY = 0x02u,                  				// 系统运行
    SYS_FLAG_STOP = 0x03u,                     				// 系统停止
    SYS_FLAG_SHUTDOWN = 0x04u,                   			// 系统关闭
}sys_flag_t;

typedef enum{
	SYS_STE_NO_ERR = 0x00u,									// 系统无错误

	SYS_STE_VOLTAGE_LOW = 0x01u,							// 电池电压低
	SYS_STE_VOLTAGE_HIGH = 0x02u,							// 电池电压高

	SYS_STE_MOTOR_1_ERR = 0x10u,							// 电机一故障
	SYS_STE_MOTOR_2_ERR = 0x11u,							// 电机二故障
	SYS_STE_MOTOR_3_ERR = 0x12u,							// 电机三故障
	SYS_STE_MOTOR_4_ERR = 0x13u,							// 电机四故障

	SYS_STE_SERVO_1_ERR = 0x20u,							// 舵机一故障
	SYS_STE_SERVO_2_ERR = 0x21u,							// 舵机二故障
	SYS_STE_SERVO_3_ERR = 0x22u,							// 舵机三故障
	SYS_STE_SERVO_4_ERR = 0x23u,							// 舵机四故障
}sys_state_t;

typedef struct{
	float X;                                    			// X轴数据
	float Y;                                      			// Y轴数据
	float Z;                                       			// Z轴数据
}geometry_t;

typedef enum{
	POWER_SUPPLY_STATUS_UNKNOWN = 0x00u,             		// 未知
	POWER_SUPPLY_STATUS_CHARGING = 0x01u,              		// 充电中
	POWER_SUPPLY_STATUS_DISCHARGING = 0x02u,             	// 放电中
	POWER_SUPPLY_STATUS_NOT_CHARGING = 0x03u,            	// 没有充电
	POWER_SUPPLY_STATUS_FULL = 0x04u,                    	// 电量已满
}power_supply_status_t;

typedef enum{
	POWER_SUPPLY_HEALTH_UNKNOWN = 0x00u,                	// 未知
	POWER_SUPPLY_HEALTH_GOOD = 0x01u,                      	// 电源健康
	POWER_SUPPLY_HEALTH_OVERHEAT = 0x02u,              		// 电源过热
	POWER_SUPPLY_HEALTH_DEAD = 0x03u,                      	// 电源损坏
	POWER_SUPPLY_HEALTH_OVERVOLTAGE = 0x04u,               	// 电压过大
	POWER_SUPPLY_HEALTH_UNSPEC_FAILURE = 0x05u,       		// 未知错误
	POWER_SUPPLY_HEALTH_COLD = 0x06u,                     	// 电源过冷
	POWER_SUPPLY_HEALTH_WATCHDOG_TIMER_EXPIRE = 0x07u,   	// 看门狗定时器到期
	POWER_SUPPLY_HEALTH_SAFETY_TIMER_EXPIRE = 0x08u,    	// 安全运行时间到期
}power_supply_health_t;

typedef enum{
	POWER_SUPPLY_TECHNOLOGY_UNKNOWN = 0x00u,               	// 未知
	POWER_SUPPLY_TECHNOLOGY_NIMH = 0x01u,               	// 镍氢电池
	POWER_SUPPLY_TECHNOLOGY_LION = 0x02u,               	// 锂离子电池
	POWER_SUPPLY_TECHNOLOGY_LIPO = 0x03u,              		// 聚合物电池
	POWER_SUPPLY_TECHNOLOGY_LIFE = 0x04u,                 	// 磷酸铁锂电池
	POWER_SUPPLY_TECHNOLOGY_NICD = 0x05u,                	// 镍镉电池
	POWER_SUPPLY_TECHNOLOGY_LIMN = 0x06u,                	// 锰酸锂电池
}power_supply_technology_t;

typedef struct{
	float voltage;                                			// 当前电池电压
	float current;                                         	// 当前放电电流
	float charge;                                          	// 当前电池容量
	float capacity;                                    		// 前次电池容量
	float design_capacity;                             		// 设计电池容量
	float percentage;                                 		// 电量百分比
	power_supply_status_t     power_supply_status;         	// 电池充电状态
	power_supply_health_t     power_supply_health;       	// 电池健康状态
	power_supply_technology_t power_supply_technology;   	// 电池类型
	uint8_t present;                                   		// 电池连接状态
}battery_t;

typedef struct{
	uint8_t t1_H;											// 任务一：子任务一
	uint8_t t1_M;											// 任务一：子任务二
	uint8_t t1_L;											// 任务一：子任务三

	uint8_t t2_H;											// 任务二：子任务一
	uint8_t t2_M;											// 任务二：子任务二
	uint8_t t2_L;											// 任务二：子任务三

	uint8_t reserve[2];
}qrcode_t;

typedef struct{
	uint8_t target_id;
	uint8_t reserve[3];

	float target_x;
	float target_y;
	float target_w;
	float target_h;
}color_t;

typedef struct{
	uint8_t state;                                       	// 相机连接状态
	uint8_t reserve[3];
	qrcode_t qrcode;										// 二维码结果
	color_t color;											// 颜色识别结果
}camera_t;

typedef struct{
	uint8_t state;                                       	// IMU连接状态
}imu_t;

typedef struct{
	float fPrePOS;                                    		// 预设角度
	float fPreSPD;                                      	// 预设速度
	float fPreTOR;                                       	// 预设力矩

	float fActPOS;                                      	// 实际角度
	float fActSPD;                                      	// 实际速度
	float fActTOR;                                      	// 实际力矩
}motor_t;

typedef struct{
	uint8_t c_task;											// 当前执行的任务（任务一，任务二）
	uint8_t c_point;										// 当前执行任务中的子任务（子任务一，子任务二，子任务三）


}task_t;

typedef struct{
	geometry_t linear_velocity;								// 运动线速度
	geometry_t angular_velocity;							// 运动角速度
}twist_t;

typedef struct{
	geometry_t position;									// 运动位移量
	geometry_t angular;										// 运动角度量
}pose_t;

typedef struct{
	pose_t pose;											// 位姿反馈量
	motor_t motor[PHY_MOTOR_MAX];							// 挂载的电机设备
	uint8_t mobile_type;									// 底盘类型
	uint8_t state;                                      	// 底盘状态
	uint8_t reserve[2];										// 4字节对齐保留位
}mobile_t;

typedef struct{
	pose_t pose;											// 位姿反馈量
	motor_t servo[PHY_SERVO_MAX];							// 挂载的舵机设备
	uint8_t manipulator_type;								// 机械臂类型
	uint8_t state;                                    		// 机械臂状态
	uint8_t reserve[3];										// 4字节对齐保留位
	int gripper;											// 末端夹持器
}manipulator_t;

typedef struct{
	uint32_t idx;                         					// 设备编号
	task_t	run_task;										// 运行的任务
	battery_t battery;                       				// 挂载的电池设备
	mobile_t mobile;            							// 挂载的底盘设备
	manipulator_t manipulator;								// 挂载的机械臂设备
	camera_t camera;										// 挂载的视觉设备
	imu_t imu;												// 挂载的姿态传感器设备
	uint8_t state;											// 设备状态
	uint8_t reserve[2];										// 4字节对齐保留位
}xbot_t;


/* Exported variables --------------------------------------------------------*/
extern UART_HandleTypeDef									IMU_PORT;
extern UART_HandleTypeDef									SERVO_PORT;
extern UART_HandleTypeDef									DEBUG_PORT;

extern ADC_HandleTypeDef									ADC_PORT_PWR;

extern CAN_HandleTypeDef 									CAN_PORT;

extern osMessageQueueId_t 									servoReplyHandle;

extern xbot_t 												xbot;	// 设备数据结构

/* Exported functions ------------------------------------------------------- */
#define MY_DEBUG											1
#if MY_DEBUG
#define XDEBUG(format, ...) printf(format, ##__VA_ARGS__)
#else
#define XDEBUG(format,...)
#endif

void xbot_startup(void);

float deg2rad(float x);
float rad2deg(float x);
void float2byte(float floatNum, unsigned char *byteArry);
float byte2float(unsigned char* byteArray);
float target_limit(float insert,float low,float high);
void delay_us(uint16_t tim);

#endif /* INTERFACELIBS_PLATFORM_H_ */
